#include <iostream>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <errno.h>
#include <time.h>   

#include "qtts.h"
#include "msp_cmn.h"
#include "msp_errors.h"

#include "ros/ros.h"
#include "std_msgs/String.h"

#include <sstream>
#include <sys/types.h>
#include <sys/stat.h>
#include <geometry_msgs/Twist.h>  


#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>



#define goal_N 3
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
move_base_msgs::MoveBaseGoal goal[goal_N];

int goal_number = 3;
geometry_msgs::Twist twist;
ros::Publisher cmd_vel_pub;
std_msgs::String string_msg;
ros::Publisher tts_text_pub;
int cmd_type = 0;

void iattextCallback(const std_msgs::String::ConstPtr& msg)
{
    char cmd[2000];
    const char* text;
       //tell the action client that we want to spin a thread by default

    // 第一个待发送的 目标点 在 map 坐标系下的坐标位置----客厅
  goal[0].target_pose.pose.position.x = 0;
  goal[0].target_pose.pose.position.y = 0;
  goal[0].target_pose.pose.orientation.w = 0.2;
    // 第二个待发送的 目标点 在 map 坐标系下的坐标位置----书房
  goal[1].target_pose.pose.position.x = -3;
  goal[1].target_pose.pose.position.y = 2;
  goal[1].target_pose.pose.orientation.w = 1;  
    // 第三个待发送的 目标点 在 map 坐标系下的坐标位置---
  goal[3].target_pose.pose.position.x = 3;
  goal[3].target_pose.pose.position.y =  -2;
  goal[3].target_pose.pose.orientation.w = 0.2;  
   //tell the action client that we want to spin a thread by default
    MoveBaseClient ac("move_base", true);
        //wait for the action server to come up
    while(!ac.waitForServer(ros::Duration(5.0)))
    {
        ROS_INFO("Waiting for the move_base action server to come up");
    }
    std::string dataString = msg->data;
    if(dataString.find("嗯") != dataString.npos)
    {
        char forwordString[40] = "开始前进";
        text = forwordString;
        std::cout<<text<<std::endl;
        cmd_type = 1;
         
         goal[goal_N -goal_number].target_pose.header.frame_id = "map";
         goal[goal_N-goal_number].target_pose.header.stamp = ros::Time::now();
        ac.sendGoal(goal[goal_N -goal_number]);
        ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
         ac.waitForResult();
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
            ROS_INFO("The NO. %d Goal achieved success !!!", goal_N-goal_number );
        }else{ROS_WARN("The NO. %d Goal Planning Failed for some reason",goal_N-goal_number); }
    }
    else if(dataString.find("嗯") != dataString.npos)
    { 
        goal_number =2;
        char backwordString[40] = "开始后退";
        text = backwordString;
        std::cout<<text<<std::endl;
        cmd_type = 2;

         goal[goal_N -goal_number].target_pose.header.frame_id = "map";
         goal[goal_N-goal_number].target_pose.header.stamp = ros::Time::now();
        ac.sendGoal(goal[goal_N -goal_number]);
        ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
         ac.waitForResult();
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
            ROS_INFO("The NO. %d Goal achieved success !!!", goal_N-goal_number );
        }else{ROS_WARN("The NO. %d Goal Planning Failed for some reason",goal_N-goal_number); }
    }
    else if(dataString.find("嗯") != dataString.npos)
    {
        char leftString[40] = "开始左转";
        text = leftString;
        std::cout<<text<<std::endl;
        cmd_type = 3;
         goal_number =1;
         goal[goal_N -goal_number].target_pose.header.frame_id = "map";
         goal[goal_N-goal_number].target_pose.header.stamp = ros::Time::now();
        ac.sendGoal(goal[goal_N -goal_number]);
        ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
         ac.waitForResult();
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED){
            ROS_INFO("The NO. %d Goal achieved success !!!", goal_N-goal_number );
        }else{ROS_WARN("The NO. %d Goal Planning Failed for some reason",goal_N-goal_number); }
    }
    else if(dataString.find("右转") != dataString.npos)
    {
        char rightdString[40] = "开始右转";
        text = rightdString;
        std::cout<<text<<std::endl;
        cmd_type = 4;
    }
    else if(dataString.find("停止") != dataString.npos)
    {
        char stopString[40] = "停止!";
        text = stopString;
        std::cout<<text<<std::endl;
        cmd_type = 5;
    }
    else
    {
        ROS_WARN("unrecognized command");
        text = msg->data.c_str();
    }
}

int main(int argc, char* argv[])
{
    setlocale(LC_ALL,"");
    char forwordString[40] = "开始前进";
    ros::init(argc,argv,"voice_control");
    ros::NodeHandle n;
    ros::Subscriber iat_text_sub =n.subscribe("ext", 1000,iattextCallback); //subscribe voice to text reault
    ROS_INFO("Wait Command...");
    ros::Rate loop_rate(50);
    while(ros::ok()){
        if(cmd_type==1){
        ROS_INFO("定位");
         }
        ros::spinOnce();
        loop_rate.sleep();
    }

exit:
	return 0;
}





// void dow(const std_msgs::String::ConstPtr& msg_s)
// {
//       ROS_INFO("%s",(*msg_s).data.c_str());
//       if((*msg_s).data.c_str()=="嗯")
//       {
//             goal_number=3;
//       }
//    ;
// }
// int main(int argc, char *argv[])
// {

//     setlocale(LC_ALL,"");
//     ros::init(argc, argv, "demo_simple_goal");
//     ros::NodeHandle nh;
//     ros::Subscriber pub = nh.subscribe("ext",10,&dow);
 

//         switch( (goal_N - goal_number) ){
//             case 0:
//                      goal[goal_N -goal_number].target_pose.header.frame_id = "map";
//                      goal[goal_N-goal_number].target_pose.header.stamp = ros::Time::now();
//                      ac.sendGoal(goal[goal_N -goal_number]);
//                      ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
//                 break;
//             case 1:
//                      goal[goal_N -goal_number].target_pose.header.frame_id = "map";
//                      goal[goal_N -goal_number].target_pose.header.stamp = ros::Time::now();
//                      ac.sendGoal(goal[goal_N -goal_number]);
//                      ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
//                 break;
//             case 2:
//                      goal[goal_N -goal_number].target_pose.header.frame_id = "map";
//                      goal[goal_N -goal_number].target_pose.header.stamp = ros::Time::now();
//                      ac.sendGoal(goal[goal_N -goal_number]);
//                      ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
//                 break;
//             case 3:
//                      goal[goal_N -goal_number].target_pose.header.frame_id = "map";
//                      goal[goal_N -goal_number].target_pose.header.stamp = ros::Time::now();
//                      ac.sendGoal(goal[goal_N -goal_number]);
//                      ROS_INFO("Send NO. %d Goal !!!", goal_N-goal_number );
//                 break;
//             default:
//                 break;
//         }


//     ros::spin();
//     return 0;
// }



